import rclpy
from geometry_msgs.msg import PoseStamped, Pose
from mnav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternion, quaternion_from_euler
from rclpy.duration import Duration
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import time
from action_msgs.msg import GoalStatusArray,GoalStatus
from lifecycle_msgs.srv import GetState
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy


class NavigatorNode(BasicNavigator):
    def __init__(self, node_name='navigator'):
        super().__init__(node_name)
        # 导航相关定义
        self.declare_parameter('initial_point', [0.0, 0.0, 0.0])
        self.declare_parameter('target_points', [0.0, 0.0, 0.0, 1.0, 1.0, 1.57])
        self.initial_point_ = self.get_parameter('initial_point').value
        self.target_points_ = self.get_parameter('target_points').value
        # 实时位置获取 TF 相关定义
        self.buffer_ = Buffer(cache_time=Duration(seconds=30))
        self.listener_ = TransformListener(self.buffer_, self,spin_thread=True)
        # self.nav_through_pose_status_sub = self.create_subscription(
        #     GoalStatusArray,
        #     'navigate_through_poses/_action/status',
        #     self.nav_through_poes_status_callback,
        #     10)
        self.custom_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=10  # 您可以根据实际需要调整深度
        )
        self.nav_pose_status_sub = self.create_subscription(
            GoalStatusArray,
            '/navigate_to_pose/_action/status',
            self.nav_through_poes_status_callback,
            self.custom_qos)
        self.nav_status = 'wait'
    
    def nav_through_poes_status_callback(self, msg):
        status_code = msg.status_list[-1].status
        if status_code == GoalStatus.STATUS_EXECUTING:
            self.nav_status = "active"
        elif status_code == GoalStatus.STATUS_SUCCEEDED:
            self.nav_status = "reached"
        elif status_code == GoalStatus.STATUS_CANCELED:
            self.nav_status = "canceled"
        elif status_code == GoalStatus.STATUS_ABORTED:
            self.nav_status = "aborted"
        elif status_code == GoalStatus.STATUS_UNKNOWN:
            self.nav_status = "unknown"
        else:
            self.nav_status = "inactive"
        self.get_logger().info(f'current state {self.nav_status}')

    def get_pose_by_xyyaw(self, x, y, yaw):
        """
        通过 x,y,yaw 合成 PoseStamped
        """
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.pose.position.x = x
        pose.pose.position.y = y
        rotation_quat = quaternion_from_euler(0, 0, yaw)
        pose.pose.orientation.x = rotation_quat[0]
        pose.pose.orientation.y = rotation_quat[1]
        pose.pose.orientation.z = rotation_quat[2]
        pose.pose.orientation.w = rotation_quat[3]
        return pose

    def init_robot_pose(self,x,y,angle):
        """
        初始化机器人位姿
        """
        # 从参数获取初始化点
        try:
            self.setInitialPose(self.get_pose_by_xyyaw(x,y,angle))
            # 等待直到导航激活
            # self.waitUntilNav2Active()
        except Exception as e:
            print(e)
            print("初始化机器人位姿失败")
            return False


    def getActivateState(self):
        try:
            state = 'inactive'
            # if not self.initial_pose_received:
            #     self.info('Setting initial pose')
            #     # self._setInitialPose()
            #     self.info('Waiting for amcl_pose to be received')
            #     return state

            # 非阻塞地检查所需服务的状态
            services = ['/bt_navigator/get_state']
            for service in services:
                state_client = self.create_client(GetState, service)
                # start_wait = time.time()
                if not state_client.wait_for_service(timeout_sec=1.0):
                    # self.info(f'{service} service not available yet.')
                    # if time.time() - start_wait > 1.0:
                    self.error(f'{service} service not available after waiting for 1 seconds.')
                    return 'inactive'
                
                req = GetState.Request()
                future = state_client.call_async(req)
                rclpy.spin_until_future_complete(self, future,timeout_sec=2.0)
                if future.done() is not None:
                    response = future.result()
                    self.info(f'response {response} state response...')
                    if response and response.current_state.label == 'active':
                        state = 'wait'  # 只有当所有检查的服务均为活跃时，才更新状态为活跃
                    else:
                        return 'inactive'  # 如果任一服务的状态不是活跃，则返回不活跃
                else:
                    self.info(f'wait for {service} state response...')
        except Exception as e:
            self.error(f'error:{e}')
            return 'unkonw'

        return state


    def get_task_status(self):
        """
        获取任务状态
        """
        if self.isTaskComplete():
            return 'wait'
        result = self.getResult()
        if result == TaskResult.SUCCEEDED:
            return 'succeeded'
        elif result == TaskResult.CANCELED:
            return 'canceled'
        elif result == TaskResult.FAILED:
            return 'failed'
        else:
            return 'unknown'
        
    def get_target_points(self):
        """
        通过参数值获取目标点集合        
        """
        points = []
        self.target_points_ = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points_)/3)):
            x = self.target_points_[index*3]
            y = self.target_points_[index*3+1]
            yaw = self.target_points_[index*3+2]
            points.append([x, y, yaw])
            self.get_logger().info(f'获取到目标点: {index}->({x},{y},{yaw})')
        return points

    def go_to_pose(self,target_pose):
        result = self.goToPose(target_pose)
        return result
    
    def go_to_poses(self,poses):
        result = self.goThroughPoses(poses)
        return result

    def cancle(self):
        self.cancelTask()

    def nav_to_pose(self, target_pose):
        """
        导航到指定位姿
        """
        # self.waitUntilNav2Active()
        result = self.goToPose(target_pose)
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            if feedback:
                self.get_logger().info(f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达')
        # 最终结果判断
        result = self.getResult()
        if result == TaskResult.SUCCEEDED:
            self.get_logger().info('导航结果：成功')
        elif result == TaskResult.CANCELED:
            self.get_logger().warn('导航结果：被取消')
        elif result == TaskResult.FAILED:
            self.get_logger().error('导航结果：失败')
        else:
            self.get_logger().error('导航结果：返回状态无效')

    def get_nav_state(self):
        pass
    def get_current_pose(self,timeout=0,map_frame='map',base_frame='base_link'):
        """
        通过TF获取当前位姿
        """
        start_time = time.time()
        if rclpy.ok():
            try:
                tf = self.buffer_.lookup_transform(
                    map_frame, base_frame, rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=0.1))
                transform = tf.transform
                rotation_euler = euler_from_quaternion([
                    transform.rotation.x,
                    transform.rotation.y,
                    transform.rotation.z,
                    transform.rotation.w
                ])
                self.get_logger().info(
                    f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}')
                return transform,rotation_euler
            except Exception as e:
                self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')
            if time.time()-start_time>timeout:
                return None,None
        return None,None
def main():
    rclpy.init()
    patrol = PatrolNode()
    patrol.speach_text(text='正在初始化位置')
    patrol.init_robot_pose()
    patrol.speach_text(text='位置初始化完成')

    while rclpy.ok():
        for point in patrol.get_target_points():
            x, y, yaw = point[0], point[1], point[2]
            # 导航到目标点
            target_pose = patrol.get_pose_by_xyyaw(x, y, yaw)
            patrol.speach_text(text=f'准备前往目标点{x},{y}')
            patrol.nav_to_pose(target_pose)
            patrol.speach_text(text=f"已到达目标点{x},{y},准备记录图像")
            patrol.record_image()
            patrol.speach_text(text=f"图像记录完成")
    rclpy.shutdown()

if __name__ == '__main__':
    main()